#include "menu.h"
PTmenu Current_menu;         //当前菜单
PTmenuitem psMenuItem;       //菜单里的菜单项
PTmenuitem psCurrentMenuItem;//当前菜单项
PTmenu psPrevMenu[10];       //记录之前的菜单
unsigned char ItemNumb[10];        
unsigned char g_nMenuLevel = 0;              //所在菜单页等级
unsigned char g_MenuItemIndex = 0;           //菜单项索引
unsigned char dis_buff[10];
///*------------------------------ Menu level 2 -------------------------------*/
TMenuOperation STMenuOperation={

  NO_INPUT,
	 UpFunc,
   DownFunc,
  SelFunc,
  ReturnFunc,
	

};
//test************************************//
unsigned short kp=0,kd=0,ki=0;
//****************************************//
Tmenuitem Angle_PIDItems[] = {
    {"Angle_Kp ADD", Angle_kp_add, IdleFunc, NULL},
    {"Angle_Kp SUB", Angle_kp_sub, IdleFunc, NULL},
    {"Angle_Kd ADD", Angle_kd_add, IdleFunc, NULL},
    {"Angle_Kd SUB", Angle_kd_sub, IdleFunc, NULL},
    {"Save", Angle_save, IdleFunc, NULL}
		};
Tmenu Angle_PID = {"Angle PID", Angle_PIDItems, COUNT_OF(Angle_PIDItems)};

Tmenuitem Velocity_PIDItems[] = {
    {"Veloc_Kp ADD", Velocity_kp_add, IdleFunc, NULL},
    {"Veloc_Kp SUB", Velocity_kp_sub, IdleFunc, NULL},
    {"Veloc_Ki ADD", Velocity_ki_add, IdleFunc, NULL},
    {"Veloc_Ki SUB", Velocity_ki_sub, IdleFunc, NULL},
    {"Save", Velocity_save, IdleFunc, NULL}
		};
Tmenu Velocity_PID = {"Velocity PID", Velocity_PIDItems, COUNT_OF(Velocity_PIDItems)};

Tmenuitem Angle_medItems[] = {

    {"Angle_med_add", Angle_med_add, IdleFunc, NULL},
		{"Angle_med_sub", Angle_med_sub, IdleFunc, NULL},
    {"Save", Angle_med_save, IdleFunc, NULL}};
Tmenu Angle_med = {"Angle med", Angle_medItems, COUNT_OF(Angle_medItems)};

Tmenuitem N20_SetItems[] = {
    {"N20_Kp ADD", N20_kp_add, IdleFunc, NULL},
    {"N20_Kp SUB", N20_kp_sub, IdleFunc, NULL},
		{"N20_Ki ADD", N20_ki_add, IdleFunc, NULL},
    {"N20_Ki SUB", N20_ki_sub, IdleFunc, NULL},
    {"Save", N20_save, IdleFunc, NULL}};
Tmenu N20_Set = {"N20 PID Set", N20_SetItems, COUNT_OF(N20_SetItems)};

Tmenuitem N20_VelocitySetItems[] = {
   	{"med_V_ADD", N20_med_add, IdleFunc, NULL},
    {"med_V_SUB", N20_med_sub, IdleFunc, NULL},
    {"Save", N20_med_save, IdleFunc, NULL}};
 Tmenu N20_Velocity = {"Velocity Set", N20_VelocitySetItems, COUNT_OF(N20_VelocitySetItems)};

 Tmenuitem Show_ParamsItems[] = {
    {"Show_Params", Open_params, Close_params, NULL}};
 
 Tmenu Show_Params = {"   Show_Params", Show_ParamsItems, COUNT_OF(Show_ParamsItems)};
///*------------------------------ Menu level 1 -------------------------------*/
//************************************主菜单**********************************//
Tmenuitem MainMenuitems[]=
{

    {"Angle_PID",   IdleFunc, IdleFunc, &Angle_PID},     //PID
    {"Velocity_PID",  IdleFunc, IdleFunc, &Velocity_PID},    //PID
    {"Angle_med",   IdleFunc, IdleFunc, &Angle_med},     //
    {"N20_PID",   IdleFunc, IdleFunc, &N20_Set},     //
    {"N20_Velocity",IdleFunc, IdleFunc, &N20_Velocity},      //
    {"Show_Params",Open_params, Close_params, NULL},      //
};

Tmenu MainMenu={"Main Menu",  MainMenuitems, COUNT_OF(MainMenuitems),};
 

//*************************************************************************************//

void IdleFunc(void)
{
    return;
}

void Menu_Init(void)
{
  Current_menu=&MainMenu;   
  psPrevMenu[g_nMenuLevel] = Current_menu;
	psMenuItem=MainMenuitems;
	memset(ItemNumb,0,sizeof(ItemNumb));
	
};

void  DisplayMenu(void)
{
 unsigned char line = 0, index = 0;
  PTmenuitem psMenuItem2;	
   LCD_Fill(0,0,LCD_W,LCD_H,BLACK);
  LCD_ShowString(20,line, Current_menu->name,LBBLUE,BLACK,16,0);
  line++;
	while ((index < (Current_menu->menuitem_number))&&(index < maxlines))
	{
			psMenuItem2 = &(Current_menu->PTmenuitems[index]);
			LCD_ShowString(0,line*ITEM_W, psMenuItem2->name,WHITE,BLACK,16,0);
			line++;
			index++;
	} 
	if (Current_menu == &Angle_PID) //
    {
     sprintf((char*)dis_buff, "KP: %.2f ", Wheel_Pid_Control.Angle_KP);
      	LCD_ShowString(0,(COUNT_OF(Angle_PIDItems)+2)*ITEM_W,dis_buff,BRRED,BLACK,16,0);
     sprintf((char*)dis_buff, "Kd: %.2f", Wheel_Pid_Control.Angle_KD);
     	LCD_ShowString(0,(COUNT_OF(Angle_PIDItems)+3)*ITEM_W, dis_buff,BRRED,BLACK,16,0);
       
    }
		if (Current_menu == &Velocity_PID) //
    {
     sprintf((char*)dis_buff, "KP: %.2f ", Wheel_Pid_Control.velocity_KP);
      	LCD_ShowString(0,(COUNT_OF(Angle_PIDItems)+2)*ITEM_W,dis_buff,BRRED,BLACK,16,0);
     sprintf((char*)dis_buff, "KI: %.2f", Wheel_Pid_Control.velocity_KI);
     	LCD_ShowString(0,(COUNT_OF(Angle_PIDItems)+3)*ITEM_W, dis_buff,BRRED,BLACK,16,0);
       
    }
	if (Current_menu == &Angle_med) //
    {
     sprintf((char*)dis_buff, "med: %.2f ", Wheel_Pid_Control.med_angle);
      LCD_ShowString(0,(COUNT_OF(Angle_medItems)+2)*ITEM_W,dis_buff,BRRED,BLACK,16,0);
    
       
    }
		if (Current_menu == &N20_Set) //
    {
   sprintf((char*)dis_buff, "KP: %.2f ", N20_Operation.pid_param->kp);
      	LCD_ShowString(0,(COUNT_OF(N20_SetItems)+2)*ITEM_W,dis_buff,BRRED,BLACK,16,0);
     sprintf((char*)dis_buff, "KI: %.2f", N20_Operation.pid_param->ki);
     	LCD_ShowString(0,(COUNT_OF(N20_SetItems)+3)*ITEM_W, dis_buff,BRRED,BLACK,16,0);     
    }
		if (Current_menu == &N20_Velocity) //
    {
     sprintf((char*)dis_buff, "med: %.2f ", N20_Operation.pid_param->med);
      LCD_ShowString(0,(COUNT_OF(N20_VelocitySetItems)+2)*ITEM_W,dis_buff,BRRED,BLACK,16,0); 
       
    }
		
		LCD_MENU_drawRectagnleFill((g_MenuItemIndex + 1) * ITEM_H, (g_MenuItemIndex + 2) * ITEM_H,RED);
		psMenuItem = &(Current_menu->PTmenuitems[g_MenuItemIndex]);
		LCD_ShowString(0,(g_MenuItemIndex + 1)*ITEM_W, psMenuItem->name,WHITE,RED,16,0);
};

/**
 * @brief 选中动作
*/
void SelFunc(void)
{
    psCurrentMenuItem =psMenuItem;
 
    /*存在子菜单跳转*/
    if (psMenuItem->PTsubmenu!=NULL)
    {
        g_nMenuLevel++;
        g_MenuItemIndex = ItemNumb[g_nMenuLevel];//记录索引
        /*得到当前菜单项的子菜单*/
        Current_menu = psMenuItem->PTsubmenu;
       //psMenuItem = &(Current_menu->PTmenuitems)[g_MenuItemIndex];
        DisplayMenu();
        psPrevMenu[g_nMenuLevel] = Current_menu;
    }
    /*执行菜单项函数*/
    psCurrentMenuItem->menufunc();
}
/**
 * @brief 返回动作
*/
void ReturnFunc(void)
{
    psMenuItem->updownfunc();

    if (g_nMenuLevel == 0)
        g_nMenuLevel++;

    /*得到上一级菜单*/
    Current_menu = psPrevMenu[g_nMenuLevel - 1];
   //psMenuItem = &Current_menu->PTmenuitems[0];
    ItemNumb[g_nMenuLevel] = 0;
    g_nMenuLevel--;
    g_MenuItemIndex = ItemNumb[g_nMenuLevel];

    DisplayMenu();
}

/**
 * @brief 向上
*/
void UpFunc(void)
{
    /*取消选中状态*/
    LCD_MENU_drawRectagnleFill((g_MenuItemIndex + 1) * ITEM_H, (g_MenuItemIndex + 2) * ITEM_H,BLACK);
    psMenuItem = &(Current_menu->PTmenuitems[g_MenuItemIndex]);
   	LCD_ShowString(0,(g_MenuItemIndex + 1)*ITEM_W, psMenuItem->name,WHITE,BLACK,16,0);
    /*判断下标状态*/
    if (g_MenuItemIndex > 0)
        g_MenuItemIndex--;
    else
        g_MenuItemIndex = Current_menu->menuitem_number - 1;
    /*更新选中状态*/
   	LCD_MENU_drawRectagnleFill((g_MenuItemIndex + 1) * ITEM_H, (g_MenuItemIndex + 2) * ITEM_H,RED);
    psMenuItem = &(Current_menu->PTmenuitems[g_MenuItemIndex]);
		LCD_ShowString(0,(g_MenuItemIndex + 1)*ITEM_W, psMenuItem->name,WHITE,RED,16,0);
    /*被选中的菜单*/
   ItemNumb[g_nMenuLevel] = g_MenuItemIndex;
}

/**
 * @brief 向下
*/
void DownFunc(void)
{
    /*取消选中状态*/
    LCD_MENU_drawRectagnleFill((g_MenuItemIndex + 1) * ITEM_H, (g_MenuItemIndex + 2) * ITEM_H,BLACK);
    psMenuItem = &(Current_menu->PTmenuitems[g_MenuItemIndex]);
   	LCD_ShowString(0,(g_MenuItemIndex + 1)*ITEM_W, psMenuItem->name,WHITE,BLACK,16,0);
    /*判断下标状态*/
    if (g_MenuItemIndex >= ((Current_menu->menuitem_number) - 1))
        g_MenuItemIndex = 0;
    else
        g_MenuItemIndex++;
   /*更新选中状态*/
   	LCD_MENU_drawRectagnleFill((g_MenuItemIndex + 1) * ITEM_H, (g_MenuItemIndex + 2) * ITEM_H,RED);
    psMenuItem = &(Current_menu->PTmenuitems[g_MenuItemIndex]);
		LCD_ShowString(0,(g_MenuItemIndex + 1)*ITEM_W, psMenuItem->name,WHITE,RED,16,0);
		if(g_MenuItemIndex==10)
		{
		
		}
    /*被选中的菜单*/
    ItemNumb[g_nMenuLevel] = g_MenuItemIndex;
	
}
//角度环参数
void Angle_kp_add(void)
{
Wheel_Pid_Control.Angle_KP+=20;
sprintf((char*)dis_buff, "KP: %.2f   ", Wheel_Pid_Control.Angle_KP);
 LCD_ShowString(0,(COUNT_OF(Angle_PIDItems)+2)*ITEM_W,dis_buff,BRRED,BLACK,16,0);
}
void Angle_kp_sub(void)
{
Wheel_Pid_Control.Angle_KP-=20;
sprintf((char*)dis_buff, "KP: %.2f   ", Wheel_Pid_Control.Angle_KP);
     	LCD_ShowString(0,(COUNT_OF(Angle_PIDItems)+2)*ITEM_W,dis_buff,BRRED,BLACK,16,0);
}
void Angle_kd_add(void)
{
Wheel_Pid_Control.Angle_KD+=0.1;
sprintf((char*)dis_buff, "KD: %.2f   ", Wheel_Pid_Control.Angle_KD);
LCD_ShowString(0,(COUNT_OF(Angle_PIDItems)+3)*ITEM_W,dis_buff,BRRED,BLACK,16,0);
}
void Angle_kd_sub(void)
{
Wheel_Pid_Control.Angle_KD-=0.1;
sprintf((char*)dis_buff, "KD: %.2f   ", Wheel_Pid_Control.Angle_KD);
 LCD_ShowString(0,(COUNT_OF(Angle_PIDItems)+3)*ITEM_W,dis_buff,BRRED,BLACK,16,0);
}
void Angle_save(void)
{
  sprintf((char*)dis_buff, "%.2f",Wheel_Pid_Control.Angle_KP*10);
	STM32_FLASH_Write(STM32_FLASH_SAVE_ADDR,(unsigned short*)dis_buff,10);
  sprintf((char*)dis_buff, "%.2f",Wheel_Pid_Control.Angle_KD*10);
	STM32_FLASH_Write(STM32_FLASH_SAVE_ADDR+20,(unsigned short*)dis_buff,10);
	LCD_ShowString(70,(g_MenuItemIndex + 1) *ITEM_W,"ok",BLUE,RED,16,0);
}

//速度环参数
void Velocity_kp_add(void)
{
Wheel_Pid_Control.velocity_KP+=1;
sprintf((char*)dis_buff, "KP: %.2f   ", Wheel_Pid_Control.velocity_KP);
 LCD_ShowString(0,(COUNT_OF(Velocity_PIDItems)+2)*ITEM_W,dis_buff,BRRED,BLACK,16,0);
}
void Velocity_kp_sub(void)
{
Wheel_Pid_Control.velocity_KP-=1;
sprintf((char*)dis_buff, "KP: %.2f   ", Wheel_Pid_Control.velocity_KP);
     	LCD_ShowString(0,(COUNT_OF(Velocity_PIDItems)+2)*ITEM_W,dis_buff,BRRED,BLACK,16,0);
}
void Velocity_ki_add(void)
{
Wheel_Pid_Control.velocity_KI+=0.1;
sprintf((char*)dis_buff, "KI: %.2f   ", Wheel_Pid_Control.velocity_KI);
LCD_ShowString(0,(COUNT_OF(Velocity_PIDItems)+3)*ITEM_W,dis_buff,BRRED,BLACK,16,0);
}
void Velocity_ki_sub(void)
{
Wheel_Pid_Control.velocity_KI-=0.1;
sprintf((char*)dis_buff, "KI: %.2f   ", Wheel_Pid_Control.velocity_KI);
 LCD_ShowString(0,(COUNT_OF(Velocity_PIDItems)+3)*ITEM_W,dis_buff,BRRED,BLACK,16,0);
}
void Velocity_save(void)
{
  sprintf((char*)dis_buff, "%.2f", Wheel_Pid_Control.velocity_KP*10);
	STM32_FLASH_Write(STM32_FLASH_SAVE_ADDR+40,(unsigned short*)dis_buff,10);
  sprintf((char*)dis_buff, "%.2f", Wheel_Pid_Control.velocity_KI*10);
	STM32_FLASH_Write(STM32_FLASH_SAVE_ADDR+60,(unsigned short*)dis_buff,10);
	LCD_ShowString(70,(g_MenuItemIndex + 1) *ITEM_W,"ok",BLUE,RED,16,0);
	
}
void Angle_med_sub(void)
{
Wheel_Pid_Control.med_angle-=0.1;
sprintf((char*)dis_buff, "med: %.2f   ", Wheel_Pid_Control.med_angle);
 LCD_ShowString(0,(COUNT_OF(Angle_medItems)+2)*ITEM_W,dis_buff,BRRED,BLACK,16,0);
}
void Angle_med_add(void)
{
Wheel_Pid_Control.med_angle+=0.1;
sprintf((char*)dis_buff, "med: %.2f   ", Wheel_Pid_Control.med_angle);
 LCD_ShowString(0,(COUNT_OF(Angle_medItems)+2)*ITEM_W,dis_buff,BRRED,BLACK,16,0);
}

void Angle_med_save(void)
{
  sprintf((char*)dis_buff, "%.2f", Wheel_Pid_Control.med_angle*10);
	STM32_FLASH_Write(STM32_FLASH_SAVE_ADDR+80,(unsigned short*)dis_buff,10);

	LCD_ShowString(70,(g_MenuItemIndex + 1) *ITEM_W,"ok",BLUE,RED,16,0);
	
}

//速度环参数
void N20_kp_add(void)
{
N20_Operation.pid_param->kp+=1;
sprintf((char*)dis_buff, "KP: %.2f   ", N20_Operation.pid_param->kp);
 LCD_ShowString(0,(COUNT_OF(N20_SetItems)+2)*ITEM_W,dis_buff,BRRED,BLACK,16,0);
}
void N20_kp_sub(void)
{
N20_Operation.pid_param->kp-=1;
sprintf((char*)dis_buff, "KP: %.2f   ", N20_Operation.pid_param->kp);
     	LCD_ShowString(0,(COUNT_OF(N20_SetItems)+2)*ITEM_W,dis_buff,BRRED,BLACK,16,0);
}
void N20_ki_add(void)
{
N20_Operation.pid_param->ki+=1;
sprintf((char*)dis_buff, "KI: %.2f   ", N20_Operation.pid_param->ki);
LCD_ShowString(0,(COUNT_OF(N20_SetItems)+3)*ITEM_W,dis_buff,BRRED,BLACK,16,0);
}
void N20_ki_sub(void)
{
N20_Operation.pid_param->ki-=1;
sprintf((char*)dis_buff, "KI: %.2f   ",N20_Operation.pid_param->ki);
 LCD_ShowString(0,(COUNT_OF(N20_SetItems)+3)*ITEM_W,dis_buff,BRRED,BLACK,16,0);
}
void N20_save(void)
{
  sprintf((char*)dis_buff, "%.2f", N20_Operation.pid_param->kp*10);
	STM32_FLASH_Write(STM32_FLASH_SAVE_ADDR+100,(unsigned short*)dis_buff,10);
  sprintf((char*)dis_buff, "%.2f", N20_Operation.pid_param->ki*10);
	STM32_FLASH_Write(STM32_FLASH_SAVE_ADDR+120,(unsigned short*)dis_buff,10);
	LCD_ShowString(70,(g_MenuItemIndex + 1) *ITEM_W,"ok",BLUE,RED,16,0);
	
}

void N20_med_add(void)
{
N20_Operation.pid_param->med+=1;
sprintf((char*)dis_buff, "med: %.2f   ", N20_Operation.pid_param->med);
 LCD_ShowString(0,(COUNT_OF(N20_VelocitySetItems)+2)*ITEM_W,dis_buff,BRRED,BLACK,16,0);
}
void N20_med_sub(void)
{
N20_Operation.pid_param->med-=1;
sprintf((char*)dis_buff, "med: %.2f   ", N20_Operation.pid_param->med);
LCD_ShowString(0,(COUNT_OF(N20_VelocitySetItems)+2)*ITEM_W,dis_buff,BRRED,BLACK,16,0);
}
void N20_med_save(void)
{
sprintf((char*)dis_buff, "%.2f",N20_Operation.pid_param->med*10);
	STM32_FLASH_Write(STM32_FLASH_SAVE_ADDR+140,(unsigned short*)dis_buff,10);
	LCD_ShowString(70,(g_MenuItemIndex + 1) *ITEM_W,"ok",BLUE,RED,16,0);
}
int params_flag=0;
void Open_params(void)
{
	params_flag=1;
  LCD_Fill(0,0,LCD_W,LCD_H,BLACK);
	
}
void Close_params(void)
{

	params_flag=0;
  LCD_Fill(0,0,LCD_W,LCD_H,BLACK);
	
}

void MainMenuInit(void)
{

	 LCD_Init();
   LCD_Fill(0,0,LCD_W,LCD_H,BLACK);
   KEY_EXTI_Init();	
   Menu_Init();
   DisplayMenu();
}
